Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(franka_gazebo): implement set_franka_model_configuration service #226

Merged

Conversation

rickstaa
Copy link
Contributor

@rickstaa rickstaa commented Feb 16, 2022

This PR implements a reset_joint_states service. This service can reset the joint states when pushed outside the joint limits. This can, for example, happen when Gazebo's set_model_configuration is used (see #225 for more information).

Update

This PR implements a set_franka_model_configuration, which can be used to set call Gazebo's set_model_configuration service while ensuring that the reported joint positions stay within joint limits (see #225 for more information)

@gollth
Copy link
Contributor

gollth commented Feb 17, 2022

Good idea @rickstaa! Makes total sense to have something like this in franka_gazebo.

@rickstaa rickstaa changed the title feat(franka_gazebo): implement 'reset_joint_states' service feat(franka_gazebo): implement 'set_franka_model_configuration' service Feb 17, 2022
@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch from eeaa513 to 88b3eff Compare February 18, 2022 10:36
@rickstaa
Copy link
Contributor Author

@gollth I just forced pushed a new version onto this pull request that implements the set_franka_model_configuration service instead. Please let me know what you think.

Some parts can be improved like:

  • Using a mutex lock instead of the requestedPosition_ variable to prevent racing conditions.
  • Using one the model_configuration map from the beginning of the service and modifying it along the way.
  • Break the set_franka_model_configuration service up in smaller submethods.
  • Change the login behaviour.
  • Use newer C++ features to clean up the code. The current implementation is based on c11.

However, I left these decisions up to you since they depend on the code style you want to use.

@rickstaa
Copy link
Contributor Author

@gollth I just saw that I use clamp which was included in C17. I will add a replacement for this.

@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch from 88b3eff to b28a8a7 Compare February 18, 2022 10:46
@rickstaa
Copy link
Contributor Author

@gollth Clang seems to fail but does not throw a warning about what goes wrong https://github.com/frankaemika/franka_ros/runs/5246382311?check_suite_focus=true.

@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch 2 times, most recently from 0fa2a0f to a1f7a11 Compare February 18, 2022 14:17
rickstaa added a commit to rickstaa/ros-gazebo-gym that referenced this pull request Feb 18, 2022
This commit migrates from the `reset_model_states` service to the
`set_franka_model_configuration` service. For more information see
frankaemika/franka_ros#226.
@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch from a1f7a11 to 99d6a63 Compare March 18, 2022 10:37
@rickstaa
Copy link
Contributor Author

rickstaa commented Mar 18, 2022

@Golth, @marcbone Again CI fail doesn't give any information on how I should change the syntax.

@rickstaa rickstaa changed the title feat(franka_gazebo): implement 'set_franka_model_configuration' service feat(franka_gazebo): implement set_franka_model_configuration service Sep 4, 2023
@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch 10 times, most recently from 370aecb to 2e2813f Compare September 4, 2023 21:05
@rickstaa
Copy link
Contributor Author

rickstaa commented Sep 4, 2023

@gollth, @Maverobot,

I've been working on resolving the CI errors we've encountered, but I could use some assistance in identifying what's going wrong. Here's what I've tried so far:

  1. Executing CI Commands Locally: I attempted to run the commands specified in the .github/workflows/ci.yml file on my local machine. Surprisingly, all tests were executed without any problems, yielding only warnings and no errors.

  2. Running clang-tidy-7 Locally on a File Basis: I used the command clang-tidy-7 -checks=* -header-filter=.* <FILE_NAME> to analyze the changed code locally, but I reencountered no errors.

  3. Using the vscode-cpp Plugin: I also tried the clang-tidy feature in the vscode-cpp extension, but once more, no errors were detected.

For context, I've configured my settings.json as follows:

"C_Cpp.codeAnalysis.clangTidy.headerFilter": "/opt/ros/noetic/include/*:../devel/include/*",
"C_Cpp.codeAnalysis.clangTidy.path": "/usr/bin/clang-tidy-7",

Since the CI tests appear to fail despite these local checks showing no issues, I'd appreciate your insights and suggestions on pinpointing the problem. Your assistance would be invaluable in resolving this matter.

@gollth
Copy link
Contributor

gollth commented Sep 5, 2023

@rickstaa seems like the formatting is off, try running:

catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
catkin build --no-deps --make-args format -- franka_gazebo

and see if that fixes the error by running:

catkin build --no-deps --make-args check-format -- franka_gazebo

@rickstaa

This comment was marked as outdated.

@rickstaa
Copy link
Contributor Author

rickstaa commented Sep 5, 2023

catkin build --no-deps --make-args check-format -- franka_gazebo

Ah, thanks! That makes sense. I, however, now get the following error message:

Errors     << catkin_tools_prebuild:make /home/ricks/development/work/franka_ros_ws/logs/catkin_tools_prebuild/build.make.004.log
make: *** No rule to make target 'format'.  Stop.
cd /home/ricks/development/work/franka_ros_ws/build/catkin_tools_prebuild; catkin build --get-env catkin_tools_prebuild | catkin env -si  /usr/bin/make format --jobserver-auth=3,4; cd -

It looks like the cmake folder is not found?

Ah, I found out that something went wrong during building. It was fixed after I reset/cleaned the catkin workspace 👍🏻.

This commit implements a `set_franka_model_configuration` service. This
service can be used to set the Franka configuration in the Gazebo
simulation. Under the hood, this service calls Gazebo's
'set_model_configuration' service while ensuring that the reported joint
positions stay within joint limits (see
frankaemika#225 for more
information).
@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch from 2e2813f to 61d9210 Compare September 5, 2023 09:19
@rickstaa
Copy link
Contributor Author

rickstaa commented Sep 5, 2023

Hey @gollth, it looks like the tests are now passing 😄.

Adding a simple CONTRIBUTING.md file to provide clear guidance for contributors is helpful. In my recent experience, I had to dive into the .github/workflows/ci.yml file to grasp all the necessary steps for contributing to your project. Having a CONTRIBUTING.md file would significantly enhance the user-friendliness of your repository and save contributors time when trying to contribute effectively.

This file doesn't need to be overly detailed; just a few key points could suffice:

  • Instructions on how you prefer people to create pull requests, such as whether they should open an issue first or follow specific protocols.
  • Recommendations for essential tools or dependencies that contributors should install when developing the project.
    Please guide the flags or commands contributors should use to ensure their pull requests align with your CI requirements.

Such a contribution guide could streamline the onboarding process for new contributors.

rickstaa added a commit to rickstaa/franka_ros that referenced this pull request Nov 20, 2023
rickstaa added a commit to rickstaa/franka_ros that referenced this pull request Nov 20, 2023
@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch from 6c8206c to 6608e14 Compare January 3, 2024 20:49
@rickstaa
Copy link
Contributor Author

rickstaa commented Jan 3, 2024

@gollth I've recently observed an issue stemming from the changes introduced in commit 89d2571. Specifically, after invoking either the original Gazebo set_model_configuration or the set_franka_model_configuration, the robot reverts to its initial position. This behaviour seems to be a direct consequence of the code implemented here:

effort = positionControl(*joint, joint->stop_position, period);

To address this, I've made some adjustments in commit 6608e14, rectifying this issue and simplifying the code and the related service message.

Considering that the current Gazebo service is incompatible with your package, it might be the right moment to consider merging this pull request. This update could significantly improve the functionality and reliability of the system 🤔.

I am looking forward to your thoughts on this.

@gollth
Copy link
Contributor

gollth commented Jan 3, 2024

Hi @rickstaa, I'm not working for Franka Emika anymore. Maybe @FE-EnricoSartori can take this over?

@rickstaa
Copy link
Contributor Author

rickstaa commented Jan 4, 2024

Hi @rickstaa, I'm not working for Franka Emika anymore. Maybe @FE-EnricoSartori can take this over?

Hello @gollth, congratulations on your new position 🚀! It's great that you've tagged the appropriate person for this review—just a tiny reminder: your GitHub profile still indicates that you are employed at Franka. You might want to update that information. Best of luck in your new role!

image

@BarisYazici
Copy link
Contributor

@rickstaa I can support you from Franka side. We are mostly focused on the ROS 2 support for FR3 robot, that's why ROS1 was left unsupported. Do you need me to review and merge the PR?

This commit ensures the `set_franka_model_configuration` works with the
changes made in 89d2571. It also
simplifies the `SetJointConfiguration.srv` message.
@rickstaa rickstaa force-pushed the fix_gazebo_set_model_config_problem branch from 6608e14 to bae0507 Compare January 4, 2024 10:15
@rickstaa
Copy link
Contributor Author

rickstaa commented Jan 4, 2024

catkin build --no-deps --make-args format -- franka_gazebo

@BarisYazici, I appreciate your prompt response and the assistance you provided in getting this pull request processed. Given that ROS Noetic will reach its end-of-life in May 2025, I fully understand your team's focus on ROS2 development 👍🏻. However, I believe that approving this pull request will be advantageous. It addresses an issue where your package is incompatible with the Gazebo set_model_configuration service, as discussed in this issue. This fix is already implemented in my own franka_ros fork, which I use in my projects (ros-gazebo-gym and panda-gazebo). Integrating this change into the main repository would be beneficial for the broader community. Please feel free to request any changes you think are necessary.

@BarisYazici BarisYazici merged commit 86f2254 into frankaemika:develop Jan 5, 2024
3 checks passed
@BarisYazici
Copy link
Contributor

Thank you for your contribution!! 🎆 👍

rickstaa added a commit to rickstaa/franka_ros that referenced this pull request Mar 24, 2024
This commit fixes a problem that was introduced when
https://github.com/rickstaa/franka_ros/tree/fix_gazebo_set_model_config_problem
was rebased onto the `develop` branch in frankaemika#226. The force push resulting
from this rebase unfortunatelly removed the code that setsup the
service.
@rickstaa
Copy link
Contributor Author

rickstaa commented Mar 24, 2024

Thank you for your contribution!! 🎆 👍

Hey @BarisYazici, thanks for your patience. I've just revisited the pull request and noticed that the rebase and force push inadvertently removed essential code from #226 related to the set_franka_model_configuration service. I've opened #380 to address this. Could you please review and merge it into the develop branch? My apologies for any hassle, and thanks again.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants